/*
 *
 * This file is part of the open-source SeetaFace engine, which includes three modules:
 * SeetaFace Detection, SeetaFace Alignment, and SeetaFace Identification.
 *
 * This file is part of the SeetaFace Detection module, containing codes implementing the
 * face detection method described in the following paper:
 *
 *
 *   Funnel-structured cascade for multi-view face detection with alignment awareness,
 *   Shuzhe Wu, Meina Kan, Zhenliang He, Shiguang Shan, Xilin Chen.
 *   In Neurocomputing (under review)
 *
 *
 * Copyright (C) 2016, Visual Information Processing and Learning (VIPL) group,
 * Institute of Computing Technology, Chinese Academy of Sciences, Beijing, China.
 *
 * The codes are mainly developed by Shuzhe Wu (a Ph.D supervised by Prof. Shiguang Shan)
 *
 * As an open-source face recognition engine: you can redistribute SeetaFace source codes
 * and/or modify it under the terms of the BSD 2-Clause License.
 *
 * You should have received a copy of the BSD 2-Clause License along with the software.
 * If not, see < https://opensource.org/licenses/BSD-2-Clause>.
 *
 * Contact Info: you can send an email to SeetaFace@vipl.ict.ac.cn for any problems.
 *
 * Note: the above information must be kept whenever or wherever the codes are used.
 *
 */
#include <string.h>
//#include<Windows.h>
#include<math.h>
#include "surf_feature_map.h"
#include "math_func.c"

void InitSURFFeaturePool(struct classSURFFeaturePool *p, int sample_width , int sample_height ,
	int patch_move_step_x , int patch_move_step_y , int patch_size_inc_step , int patch_min_width , int patch_min_height )
{
	p->sample_width_ = sample_width;
	p->sample_height_ = sample_height;
	p->patch_move_step_x_ = patch_move_step_x;
	p->patch_move_step_y_ = patch_move_step_y;
	p->patch_size_inc_step_ = patch_size_inc_step;
	p->patch_min_width_ = patch_min_width;
	p->patch_min_height_ = patch_min_height;
}

void SURFFeatureMapFillIntegralChannel(struct classSURFFeatureMap *p, const int32_t* src, int32_t ch) {
	int i;
	int32_t* dest = p->int_img_ + ch;
	int32_t len = p->featuremap.width_ * p->featuremap.height_;
	for ( i = 0; i < len; i++) {
		*dest = *src;
		*(dest + 2) = *src;
		dest += kNumIntChannel;
		src++;
	}
}
bool SURFFeaturePoolempty(struct classSURFFeaturePool *p)  { return (p->pool_== NULL); }
int SURFFeaturePoolsize(struct classSURFFeaturePool *p)  { return sizeof(p->pool_) / sizeof(p->pool_[0]); }

SURFFeature SURFFeaturePoolbegin(struct classSURFFeaturePool *p)  {
	return p->pool_[0];
}

SURFFeature SURFFeaturePoolend(struct classSURFFeaturePool *p)  {
	return p->pool_[SURFFeaturePoolsize(p) - 1];
}

const SURFFeature SURFFeaturePoolidx(struct classSURFFeaturePool *p, size_t idx)  {
	return p->pool_[idx];
}
void InitSURFFeatureMap(struct classSURFFeatureMap *p, bool buf_valid_reset)
{
	p->buf_valid_reset_ = buf_valid_reset;
	SURFFeatureMapInitFeaturePool(p);
}
void SURFFeatureMapSetROI(struct classSURFFeatureMap *p, const Rect  roi) {
	p->featuremap.roi_ = roi;
	if (p->buf_valid_reset_) {
		memset(p->buf_valid_, 0, malloc_usable_size(p->buf_valid_)/sizeof(int32_t) * sizeof(int32_t));
		p->buf_valid_reset_ = false;
	}
}
int32_t SURFFeatureMapGetFeatureVectorDim(struct classSURFFeatureMap *p, int32_t feat_id)
{
	return (p->feat_pool_.pool_[feat_id].num_cell_per_col *
		p->feat_pool_.pool_[feat_id].num_cell_per_row * kNumIntChannel);
}
void SURFFeaturePoolCreate(struct classSURFFeaturePool *p) {
	int lenformat = sizeof(p->format_) / sizeof(p->format_[0]);
	int poolindex = 0;
	if (p->sample_height_ - p->patch_min_height_ <= p->sample_width_ - p->patch_min_width_) {
		int i,h;
		for ( i = 0; i < lenformat; i++) {
			const SURFPatchFormat  format = p->format_[i];
			for ( h = p->patch_min_height_; h <= p->sample_height_;
				h += p->patch_size_inc_step_) {
        if (h % format.num_cell_per_col != 0 || h % format.height != 0)
          continue;
        int32_t w = h / format.height * format.width;
		if (w % format.num_cell_per_row != 0 || w < p->patch_min_width_ ||
			w > p->sample_width_)
          continue;
		SURFFeaturePoolAddAllFeaturesToPool(p, w, h, format.num_cell_per_row,
			format.num_cell_per_col, &poolindex);
      }
    }
  } else {
  		int i,w;
		for ( i = 0; i < lenformat; i++) {
		const SURFPatchFormat  format = p->format_[i];
		for ( w = p->patch_min_width_; w <= p->patch_min_width_;
			w += p->patch_size_inc_step_) {
        if (w % format.num_cell_per_row != 0 || w % format.width != 0)
          continue;
        int32_t h = w / format.width * format.height;
		if (h % format.num_cell_per_col != 0 || h < p->patch_min_height_ ||
			h > p->sample_height_)
          continue;
		SURFFeaturePoolAddAllFeaturesToPool(p, w, h, format.num_cell_per_row,
			format.num_cell_per_col, &poolindex);
      }
    }
  }
}

void SURFFeaturePoolAddPatchFormat(struct classSURFFeaturePool *p, int index, int32_t width, int32_t height,
    int32_t num_cell_per_row, int32_t num_cell_per_col) {
	int lenformat = sizeof(p->format_) / sizeof(SURFPatchFormat);
	int i;
	for ( i = 0; i < index; i++) {
	  const SURFPatchFormat  format = p->format_[i];
    if (format.height == height &&
      format.width == width &&
      format.num_cell_per_row == num_cell_per_row &&
      format.num_cell_per_col == num_cell_per_col)
      return;
  }

  SURFPatchFormat new_format;
  new_format.height = height;
  new_format.width = width;
  new_format.num_cell_per_row = num_cell_per_row;
  new_format.num_cell_per_col = num_cell_per_col;
  //p->format_.push_back(new_format);
  p->format_[index] = new_format;
}

void SURFFeaturePoolAddAllFeaturesToPool(struct classSURFFeaturePool *p, int32_t width, int32_t height,
    int32_t num_cell_per_row, int32_t num_cell_per_col,int* poolindex) {
  SURFFeature feat;
  feat.patch.width = width;
  feat.patch.height = height;
  feat.num_cell_per_row = num_cell_per_row;
  feat.num_cell_per_col = num_cell_per_col;
  int32_t x,y;
  for ( y = 0; y <= p->sample_height_ - height; y += p->patch_move_step_y_) {
    feat.patch.y = y;
    for ( x = 0; x <= p->sample_width_ - width; x += p->patch_move_step_x_) {
      feat.patch.x = x;
      //p->pool_.push_back(feat);
	  p->pool_[*poolindex] = feat;
	  *poolindex += 1;
    }
  }
}

void SURFFeatureMapCompute(struct classSURFFeatureMap *p, const uint8_t* input, int32_t width,
    int32_t height,FDHandle* handle) {
  if (input == NULL || width <= 0 || height <= 0) {
    return;  // @todo handle the error!
  }
  SURFFeatureMapReshape(p, width, height,handle);
  SURFFeatureMapComputeGradientImages(p, input);
  SURFFeatureMapComputeIntegralImages(p);
}

void SURFFeatureMapGetFeatureVector(struct classSURFFeatureMap *p, int32_t feat_id, float* feat_vec) {
	int len_feat_vec_normed = malloc_usable_size(p->feat_vec_normed_buf_[feat_id]) / sizeof(p->feat_vec_normed_buf_[feat_id][0]);
	
	if (p->buf_valid_[feat_id] == 0) {
	  
	  SURFFeatureMapComputeFeatureVector(p, SURFFeaturePoolidx(&p->feat_pool_, feat_id), p->feat_vec_buf_[feat_id]);
	  
	  
   SURFFeatureMapNormalizeFeatureVectorL2(p,p->feat_vec_buf_[feat_id],
	   p->feat_vec_normed_buf_[feat_id], len_feat_vec_normed);
    p->buf_valid_[feat_id] = 1;
    p->buf_valid_reset_ = true;
  }

  memcpy(feat_vec, p->feat_vec_normed_buf_[feat_id],
	  len_feat_vec_normed*sizeof(float));
  
}

void SURFFeatureMapInitFeaturePool(struct classSURFFeatureMap *p) {
	InitSURFFeaturePool(&p->feat_pool_,40,40,16,16,1,16,16);
	//p->feat_pool_.format_ = (SURFPatchFormat*)malloc(sizeof(SURFPatchFormat)* 5);
	SURFFeaturePoolAddPatchFormat(&p->feat_pool_, 0, 1, 1, 2, 2);
	SURFFeaturePoolAddPatchFormat(&p->feat_pool_, 1, 1, 2, 2, 2);
	SURFFeaturePoolAddPatchFormat(&p->feat_pool_, 2, 2, 1, 2, 2);
	SURFFeaturePoolAddPatchFormat(&p->feat_pool_, 3, 2, 3, 2, 2);
	SURFFeaturePoolAddPatchFormat(&p->feat_pool_, 4, 3, 2, 2, 2);
	SURFFeaturePoolCreate(&p->feat_pool_);
 int i;
  int32_t feat_pool_size = (int32_t)(SURFFeaturePoolsize(&p->feat_pool_));
 // p->feat_vec_buf_.resize(feat_pool_size);
//  p->feat_vec_normed_buf_.resize(feat_pool_size);
  p->feat_vec_buf_ = (int32_t**)malloc(sizeof(int32_t*)*feat_pool_size);
  p->feat_vec_normed_buf_ = (float**)malloc(sizeof(float*)*feat_pool_size);
  for ( i = 0; i < feat_pool_size; i++) {
	  int32_t dim = SURFFeatureMapGetFeatureVectorDim(p, i);
    //p->feat_vec_buf_[i].resize(dim);
   // p->feat_vec_normed_buf_[i].resize(dim);
	  p->feat_vec_buf_[i] = (int32_t*)malloc(sizeof(int32_t)*dim);
	  p->feat_vec_normed_buf_[i] = (float*)malloc(sizeof(float)*dim);
  }
 // p->buf_valid_.resize(feat_pool_size, 0);
  p->buf_valid_ = (int32_t*)malloc(sizeof(int32_t)*feat_pool_size);
  memset(p->buf_valid_, 0, sizeof(int32_t)*feat_pool_size);
}

void SURFFeatureMapReshape(struct classSURFFeatureMap *p, int32_t width, int32_t height,FDHandle* handle) {
  p->featuremap.width_ = width;
  p->featuremap.height_ = height;

  int32_t len = p->featuremap.width_ *p->featuremap.height_;
  /*
  p->grad_x_ = (int32_t*)malloc(sizeof(int32_t)*len);
  p->grad_y_ = (int32_t*)malloc(sizeof(int32_t)*len);
  p->int_img_ = (int32_t*)malloc(sizeof(int32_t)*len*kNumIntChannel);
  p->img_buf_ = (int32_t*)malloc(sizeof(int32_t)*len);
  */
  p->grad_x_ = (int32_t*)(handle->mem + handle->MemeryUsed);
  handle->MemeryUsed += sizeof(int32_t)*len;
  p->grad_y_ = (int32_t*)(handle->mem + handle->MemeryUsed);
  handle->MemeryUsed += sizeof(int32_t)*len;
  p->int_img_ = (int32_t*)(handle->mem + handle->MemeryUsed);
  handle->MemeryUsed += sizeof(int32_t)*len*kNumIntChannel;
  p->img_buf_ = (int32_t*)(handle->mem + handle->MemeryUsed);
  handle->MemeryUsed += sizeof(int32_t)*len;
}

void SURFFeatureMapComputeGradientImages(struct classSURFFeatureMap *p, const uint8_t* input) {
	int32_t len = p->featuremap.width_ * p->featuremap.height_;
  UInt8ToInt32(input, p->img_buf_, len);
  SURFFeatureMapComputeGradX(p, p->img_buf_);
  SURFFeatureMapComputeGradY(p, p->img_buf_);
}

void SURFFeatureMapComputeGradX(struct classSURFFeatureMap *p, const int32_t* input) {
  int32_t* dx = p->grad_x_;
  int32_t len = p->featuremap.width_ - 2;
  int32_t r;
#pragma omp parallel num_threads(SEETA_NUM_THREADS)
  {
#pragma omp for nowait
    for (r = 0; r < p->featuremap.height_; r++) {
		const int32_t* src = input + r * p->featuremap.width_;
      int32_t* dest = dx + r * p->featuremap.width_;
      *dest = ((*(src + 1)) - (*src)) << 1;
	  VectorSub(src + 2, src, dest + 1, len);
      dest += (p->featuremap.width_ - 1);
	  src += (p->featuremap.width_ - 1);
      *dest = ((*src) - (*(src - 1))) << 1;
    }
  }
}

void SURFFeatureMapComputeGradY(struct classSURFFeatureMap *p, const int32_t* input) {
  int32_t* dy = p->grad_y_;
  int32_t len = p->featuremap.width_;
  VectorSub(input + p->featuremap.width_, input, dy, len);
  VectorAdd(dy, dy, dy, len);
  int32_t r;
#pragma omp parallel num_threads(SEETA_NUM_THREADS)
  {
#pragma omp for nowait
	  for (r = 1; r < p->featuremap.height_ - 1; r++) {
		  const int32_t* src = input + (r - 1) * p->featuremap.width_;
		  int32_t* dest = dy + r * p->featuremap.width_;
		  VectorSub(src + (p->featuremap.width_ << 1), src, dest, len);
    }
  }
  int32_t offset = (p->featuremap.height_ - 1) * p->featuremap.width_;
  dy += offset;
  VectorSub(input + offset, input + offset - p->featuremap.width_,
    dy, len);
  VectorAdd(dy, dy, dy, len);
}

void SURFFeatureMapComputeIntegralImages(struct classSURFFeatureMap *p) {
	SURFFeatureMapFillIntegralChannel(p, p->grad_x_, 0);
	SURFFeatureMapFillIntegralChannel(p, p->grad_y_, 4);

  int32_t len = p->featuremap.width_ * p->featuremap.height_;
  VectorAbs(p->grad_x_, p->img_buf_, len);
  SURFFeatureMapFillIntegralChannel(p, p->img_buf_, 1);
  VectorAbs(p->grad_y_, p->img_buf_, len);
  SURFFeatureMapFillIntegralChannel(p, p->img_buf_, 5);
  SURFFeatureMapMaskIntegralChannel(p);
  SURFFeatureMapIntegral(p);
}

void SURFFeatureMapMaskIntegralChannel(struct classSURFFeatureMap *p) {
  const int32_t* grad_x = p->grad_x_;
  const int32_t* grad_y = p->grad_y_;
  __m128i dx;
  __m128i dy;
  __m128i dx_mask;
  __m128i dy_mask;
  __m128i zero = _mm_set1_epi32(0);
  __m128i xor_bits = _mm_set_epi32(0x0, 0x0, 0xffffffff, 0xffffffff);
  __m128i data;
  __m128i result;
  __m128i* src = (__m128i*)(p->int_img_);
  int32_t len = p->featuremap.width_ * p->featuremap.height_;
  int32_t i;
  for ( i = 0; i < len; i++) {
    dx = _mm_set1_epi32(*(grad_x++));
    dy = _mm_set1_epi32(*(grad_y++));
    dx_mask = _mm_xor_si128(_mm_cmplt_epi32(dx, zero), xor_bits);
    dy_mask = _mm_xor_si128(_mm_cmplt_epi32(dy, zero), xor_bits);

    data = _mm_loadu_si128(src);
    result = _mm_and_si128(data, dy_mask);
    _mm_storeu_si128(src++, result);
    data = _mm_loadu_si128(src);
    result = _mm_and_si128(data, dx_mask);
    _mm_storeu_si128(src++, result);
  }
}

void SURFFeatureMapIntegral(struct classSURFFeatureMap *p) {
  int32_t* data = p->int_img_;
  int32_t len = kNumIntChannel * p->featuremap.width_;
  int32_t r;
  // Cummulative sum by row
  for ( r = 0; r < p->featuremap.height_ - 1; r++) {
    int32_t* row1 = data + r * len;
    int32_t* row2 = row1 + len;
    VectorAdd(row1, row2, row2, len);
  }
  // Cummulative sum by column
  for ( r = 0; r < p->featuremap.height_; r++)
	  SURFFeatureMapVectorCumAdd(p, data + r * len, len, kNumIntChannel);
}

void SURFFeatureMapVectorCumAdd(struct classSURFFeatureMap *p, int32_t* x, int32_t len,
    int32_t num_channel) {
  __m128i x1;
  __m128i y1;
  __m128i z1;
  __m128i* x2 = (__m128i*)(x);
  __m128i* y2 = (__m128i*)(x + num_channel);
  __m128i* z2 = y2;
  int32_t i;
  len = len / num_channel - 1;
  for ( i = 0; i < len; i++) {
    // first 4 channels
    x1 = _mm_loadu_si128(x2++);
    y1 = _mm_loadu_si128(y2++);
    z1 = _mm_add_epi32(x1, y1);
    _mm_storeu_si128(z2, z1);
    z2 = y2;

    // second 4 channels
    x1 = _mm_loadu_si128(x2++);
    y1 = _mm_loadu_si128(y2++);
    z1 = _mm_add_epi32(x1, y1);
    _mm_storeu_si128(z2, z1);
    z2 = y2;
  }
}

void SURFFeatureMapComputeFeatureVector(struct classSURFFeatureMap *p, const SURFFeature  feat,
    int32_t* feat_vec) {
	int32_t init_cell_x = p->featuremap.roi_.x + feat.patch.x;
  int32_t init_cell_y = p->featuremap.roi_.y + feat.patch.y;
  int32_t cell_width = feat.patch.width / feat.num_cell_per_row * kNumIntChannel;
  int32_t cell_height = feat.patch.height / feat.num_cell_per_col;
  int32_t row_width = p->featuremap.width_ * kNumIntChannel;
  const int32_t* cell_top_left[kNumIntChannel];
  const int32_t* cell_top_right[kNumIntChannel];
  const int32_t* cell_bottom_left[kNumIntChannel];
  const int32_t* cell_bottom_right[kNumIntChannel];
  int* feat_val = feat_vec;
  const int32_t* int_img = p->int_img_;
  int32_t offset = 0;
  int i,j,k;
  if (init_cell_y != 0) {
    if (init_cell_x != 0) {
      const int32_t* tmp_cell_top_right[kNumIntChannel];

      // cell #1
      offset = row_width * (init_cell_y - 1) +
        (init_cell_x - 1) * kNumIntChannel;
      for ( i = 0; i < kNumIntChannel; i++) {
        cell_top_left[i] = int_img + (offset++);
        cell_top_right[i] = cell_top_left[i] + cell_width;
        cell_bottom_left[i] = cell_top_left[i] + row_width * cell_height;
        cell_bottom_right[i] = cell_bottom_left[i] + cell_width;
        *(feat_val++) = *(cell_bottom_right[i]) + *(cell_top_left[i]) -
                        *(cell_top_right[i]) - *(cell_bottom_left[i]);
        tmp_cell_top_right[i] = cell_bottom_right[i];
      }

      // cells in 1st row
      for ( i = 1; i < feat.num_cell_per_row; i++) {
        for ( j = 0; j < kNumIntChannel; j++) {
          cell_top_left[j] = cell_top_right[j];
          cell_top_right[j] += cell_width;
          cell_bottom_left[j] = cell_bottom_right[j];
          cell_bottom_right[j] += cell_width;
          *(feat_val++) = *(cell_bottom_right[j]) + *(cell_top_left[j]) -
                          *(cell_top_right[j]) - *(cell_bottom_left[j]);
        }
      }

      for ( i = 0; i < kNumIntChannel; i++)
        cell_top_right[i] = tmp_cell_top_right[i];
    } else {
      const int32_t* tmp_cell_top_right[kNumIntChannel];

      // cell #1
      offset = row_width * (init_cell_y - 1) + cell_width - kNumIntChannel;
      for ( i = 0; i < kNumIntChannel; i++) {
        cell_top_right[i] = int_img + (offset++);
        cell_bottom_right[i] = cell_top_right[i] + row_width * cell_height;
        tmp_cell_top_right[i] = cell_bottom_right[i];
        *(feat_val++) = *(cell_bottom_right[i]) - *(cell_top_right[i]);
      }

      // cells in 1st row
      for ( i = 1; i < feat.num_cell_per_row; i++) {
        for ( j = 0; j < kNumIntChannel; j++) {
          cell_top_left[j] = cell_top_right[j];
          cell_top_right[j] += cell_width;
          cell_bottom_left[j] = cell_bottom_right[j];
          cell_bottom_right[j] += cell_width;
          *(feat_val++) = *(cell_bottom_right[j]) + *(cell_top_left[j]) -
                          *(cell_top_right[j]) - *(cell_bottom_left[j]);
        }
      }

      for ( i = 0; i < kNumIntChannel; i++)
        cell_top_right[i] = tmp_cell_top_right[i];
    }
  } else {
    if (init_cell_x != 0) {
      // cell #1
      offset = row_width * (cell_height - 1) +
        (init_cell_x - 1) * kNumIntChannel;
      for ( i = 0; i < kNumIntChannel; i++) {
        cell_bottom_left[i] = int_img + (offset++);
        cell_bottom_right[i] = cell_bottom_left[i] + cell_width;
        *(feat_val++) = *(cell_bottom_right[i]) - *(cell_bottom_left[i]);
        cell_top_right[i] = cell_bottom_right[i];
      }

      // cells in 1st row
      for ( i = 1; i < feat.num_cell_per_row; i++) {
        for ( j = 0; j < kNumIntChannel; j++) {
          cell_bottom_left[j] = cell_bottom_right[j];
          cell_bottom_right[j] += cell_width;
          *(feat_val++) = *(cell_bottom_right[j]) - *(cell_bottom_left[j]);
        }
      }
    } else {
      // cell #1
      offset = row_width * (cell_height - 1) + cell_width - kNumIntChannel;
      for ( i = 0; i < kNumIntChannel; i++) {
        cell_bottom_right[i] = int_img + (offset++);
        *(feat_val++) = *(cell_bottom_right[i]);
        cell_top_right[i] = cell_bottom_right[i];
      }

      // cells in 1st row
      for ( i = 1; i < feat.num_cell_per_row; i++) {
        for ( j = 0; j < kNumIntChannel; j++) {
          cell_bottom_left[j] = cell_bottom_right[j];
          cell_bottom_right[j] += cell_width;
          *(feat_val++) = *(cell_bottom_right[j]) - *(cell_bottom_left[j]);
        }
      }
    }
  }

  // from BR of last cell in current row to BR of first cell in next row
  offset = cell_height * row_width - feat.patch.width *
    kNumIntChannel + cell_width;

  // cells in following rows
  for ( i = 1; i < feat.num_cell_per_row; i++) {
    // cells in 1st column
    if (init_cell_x == 0) {
      for ( j = 0; j < kNumIntChannel; j++) {
        cell_bottom_right[j] += offset;
        *(feat_val++) = *(cell_bottom_right[j]) - *(cell_top_right[j]);
      }
    } else {
      for ( j = 0; j < kNumIntChannel; j++) {
        cell_bottom_right[j] += offset;
        cell_top_left[j] = cell_top_right[j] - cell_width;
        cell_bottom_left[j] = cell_bottom_right[j] - cell_width;
        *(feat_val++) = *(cell_bottom_right[j]) + *(cell_top_left[j]) -
                        *(cell_top_right[j]) - *(cell_bottom_left[j]);
      }
    }

    // cells in following columns
    for ( j = 1; j < feat.num_cell_per_row; j++) {
      for ( k = 0; k < kNumIntChannel; k++) {
        cell_top_left[k] = cell_top_right[k];
        cell_top_right[k] += cell_width;

        cell_bottom_left[k] = cell_bottom_right[k];
        cell_bottom_right[k] += cell_width;

        *(feat_val++) = *(cell_bottom_right[k]) + *(cell_top_left[k]) -
                        *(cell_bottom_left[k]) - *(cell_top_right[k]);
      }
    }

    for ( j = 0; j < kNumIntChannel; j++)
      cell_top_right[j] += offset;
  }
}

void SURFFeatureMapNormalizeFeatureVectorL2(struct classSURFFeatureMap *p, const int32_t* feat_vec,
    float* feat_vec_normed, int32_t len)  {
  double prod = 0.0;
  float norm_l2 = 0.0f;
  int32_t i;
  for ( i = 0; i < len; i++)
    prod += (double)(feat_vec[i] * feat_vec[i]);
  if (prod != 0) {
    norm_l2 = (float)(sqrt(prod));
    for ( i = 0; i < len; i++)
      feat_vec_normed[i] = feat_vec[i] / norm_l2;
  } else {
    for ( i = 0; i < len; i++)
      feat_vec_normed[i] = 0.0f;
  }
}


